#include <arpa/inet.h>
#include <netinet/in.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <unistd.h>
#include <stdlib.h> 
#include <string.h>
#define BUFLEN 512
#define PORT 9930


int hmVert=-20;	// head move vertical
int hmHori=0;	// head move horisontal

int mf=0;	// move forward
int mb=0;	// move back
int tl=0;	// turn left
int tr=0;	// turn right


char servo[35];

void err(char *str)
{
    perror(str);
    exit(1);
}


void stop(){
                
	printf("Stopping\n");
	sprintf(servo,"echo 9:off  > /dev/servodrive0");
       	system(servo);
	sprintf(servo,"echo 10:off  > /dev/servodrive0");
       	system(servo);
	mf=0;
	mb=0;
}

void moveForward(){

	stop();
	if(mf==0){
		printf("Moving Forward\n");

		sprintf(servo,"echo 9:%d  > /dev/servodrive0",20);
       		system(servo);
		sprintf(servo,"echo 10:%d  > /dev/servodrive0",-20);
	       	system(servo);
		mf=1;
	}
	else{
		mf=0;
	}
}

void moveBack(){

	stop();
	if(mb==0){
		printf("Moving Back\n");

		sprintf(servo,"echo 9:%d  > /dev/servodrive0",-20);
       		system(servo);
		sprintf(servo,"echo 10:%d  > /dev/servodrive0",20);
	       	system(servo);
		mb=1;

	}
	else{
		mb=0;
	}
}	


void turnLeft(){

	stop();
	if(tl==0){
		printf("Turning Left\n");

		sprintf(servo,"echo 9:%d  > /dev/servodrive0",5);
       		system(servo);
		sprintf(servo,"echo 10:%d  > /dev/servodrive0",5);
       		system(servo);
		tl=1;
	}
	else{
		tl=0;
	}
}


void turnRight(){

	stop();
	if(tr==0){
		printf("Turning Right\n");

		sprintf(servo,"echo 9:%d  > /dev/servodrive0",-5);
       		system(servo);
		sprintf(servo,"echo 10:%d  > /dev/servodrive0",-5);
       		system(servo);
		tr=1;
	}
	else{
		tr=0;
	}
}


void headInitPos(){


	hmVert=-20;	// head move vertical
	hmHori=0;	// head move horisontal
		
	printf("Head Init Postition\n");
	sprintf(servo,"echo 11:%d  > /dev/servodrive0",-20);
       	system(servo);
	sprintf(servo,"echo 12:%d  > /dev/servodrive0",0);
       	system(servo);
}

void headMoveUP(){
		
	printf("Head UP\n");
	hmVert-=2;
	sprintf(servo,"echo 11:%d  > /dev/servodrive0",hmVert);
       	system(servo);
}

void headMoveDown(){
	printf("Head Down\n");
	hmVert+=2;
	sprintf(servo,"echo 11:%d  > /dev/servodrive0",hmVert);
       	system(servo);
}

void headTurnLeft(){
	printf("Head Left\n");
	hmHori+=2;
	sprintf(servo,"echo 12:%d  > /dev/servodrive0",hmHori);
       	system(servo);
}

void headTurnRight(){
	printf("Head Right\n");
	hmHori-=2;
	sprintf(servo,"echo 12:%d  > /dev/servodrive0",hmHori);
       	system(servo);
}



int main(void)
{
    struct sockaddr_in my_addr, cli_addr;
    int sockfd, i; 
    socklen_t slen=sizeof(cli_addr);
    char buf[BUFLEN];

    if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1)
      err("socket");
    else 
      printf("Server : Socket() successful\n");

    bzero(&my_addr, sizeof(my_addr));
    my_addr.sin_family = AF_INET;
    my_addr.sin_port = htons(PORT);
    my_addr.sin_addr.s_addr = htonl(INADDR_ANY);
    
    if (bind(sockfd, (struct sockaddr* ) &my_addr, sizeof(my_addr))==-1)
      err("bind");
    else
      printf("Server : bind() successful\n");

    while(1)
    {

	int sz=recvfrom(sockfd, buf, BUFLEN, 0, (struct sockaddr*)&cli_addr, &slen);
        if (sz==-1)
        	err("recvfrom()");
        printf("Received packet from %s:%d\nData: %s\n\n",
        inet_ntoa(cli_addr.sin_addr), ntohs(cli_addr.sin_port), buf);

	buf[sz]='\0';




	if(strcmp(buf,"mf")==0){


		//printf("Moving Forward\n");

		moveForward();

	}


	if(strcmp(buf,"mb")==0){
	
		//printf("Moving Back\n");

		moveBack();


	}
	
	if(strcmp(buf,"tl")==0){
		
		//printf("Turning Left\n");

		turnLeft();

	}

	if(strcmp(buf,"tr")==0){
		
		//printf("Turning Right\n");

		turnRight();


	}
	


	if(strcmp(buf,"hu")==0){

                //printf("Moving Head UP\n");

		headMoveUP();


        }


        if(strcmp(buf,"hd")==0){

                //printf("Moving Head  Down\n");
	
		headMoveDown();


        }

        if(strcmp(buf,"hl")==0){

                //printf("Turning Head Left\n");

		headTurnLeft();

        }

        if(strcmp(buf,"hr")==0){

                //printf("Turning Head Right\n");

		headTurnRight();


        }
        

	if(strcmp(buf,"ip")==0){


		headInitPos();

	}

	if(strcmp(buf,"stop")==0){

                //printf("Stopping\n");

		stop();


        }
	
	

    }

    close(sockfd);
    return 0;
}

